#include "ecv_filter_convhough.h"


/*
 * Gd includes
 */

#include "gd.h"
#include "gdfontt.h"
#include "gdfontl.h"
#include <memory.h>

#include <stdio.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <ctype.h>
#include <unistd.h>

G_DEFINE_TYPE (EcvFilter_Convhough, ecv_filter_convhough, ECV_TYPE_FILTER);

#define ECV_FILTER_CONVHOUGH_GET_PRIVATE(obj) (G_TYPE_INSTANCE_GET_PRIVATE ((obj), ECV_TYPE_FILTER_CONVHOUGH, EcvFilter_ConvhoughPrivate))

struct _EcvFilter_ConvhoughPrivate
{
  int hspw;
  int hsph;
  unsigned int * hsp;
  char max;
  double * costable;
  double * sintable;
  GHashTable * lines;
};

enum
{
  PROP_0

};

/*void houghsethsp(int * hsp, int ro, int theta)
{
  
}*/


void convhoughremovenoise(EcvFilter_Convhough *self, GHashTable * table)
{
  GHashTableIter iter;
  dot * newdot;
  int * key;
  dot * value;
  
  memset(self->priv->hsp,0,self->priv->hspw*self->priv->hsph*sizeof(int));
  g_hash_table_iter_init(&iter, table);
  while (g_hash_table_iter_next (&iter, (void**)&key, (void**)&value)) 
    {
      self->priv->hsp[value->x+value->y*self->priv->hspw]=0xFF;
    }
}

void convhoughfindclustersradius (EcvFilter_Convhough *self, int radius)
{
  int * key;
  dot * value;
  GHashTableIter iter;
  if (self->priv->lines!=NULL)
    {
      g_hash_table_iter_init(&iter, self->priv->lines);
      while (g_hash_table_iter_next (&iter, (void**)&key, (void**)&value)) 
	{
	  free(value);
	}

      g_hash_table_destroy(self->priv->lines);
    }

  self->priv->lines = g_hash_table_new(g_int_hash, g_int_equal);
  unsigned int theta;
  unsigned int ro;
  unsigned int id=0;
  double x;
  double y;
  double r=radius;
  dot * newdot;
  unsigned int px;
  int isnear=0;
	  
  for (theta=0;theta<self->priv->hsph;theta=theta+1)
    {
      for (ro=0;ro<self->priv->hspw;ro=ro+1)
	{
	  isnear = 0;
	  px = self->priv->hsp[ro+theta*self->priv->hspw];
	  if (px!=0)
	    {
	      g_hash_table_iter_init(&iter, self->priv->lines);
	      while (g_hash_table_iter_next (&iter, (void**)&key, (void**)&value)) 
		{
		  x = (double)value->x-(double)ro;
		  y = (double)value->y-(double)theta;
		  if (x*x+y*y<=r*r)
		    {
		      isnear = 1;
		    }
		}
	      if (isnear!=1)
		{
		  newdot = malloc(sizeof(dot));
		  newdot->x=ro;
		  newdot->y=theta;
		  newdot->id=id;
		  id=id+1;
		  g_hash_table_insert(self->priv->lines, &(newdot->id), newdot);
		}
	    }
	}
    }
  
  convhoughremovenoise(self,self->priv->lines);

}



void convhoughsmooth (EcvFilter_Convhough *bin)
{
  int ro;
  unsigned int theta;
  unsigned int thiscell = 0;
  unsigned int celltocheck = 0;
  for (theta=0;theta<bin->priv->hsph;theta=theta+1)
    {
      for (ro=0;ro<bin->priv->hspw;ro=ro+1)
	{
	  //check every cell around
	  thiscell = bin->priv->hsp[ro+theta*bin->priv->hspw];
	  //check previous in this 
	  celltocheck = bin->priv->hsp[ro+theta*bin->priv->hspw];

	  if (ro>0)
	    {
	      celltocheck = bin->priv->hsp[ro-1+theta*bin->priv->hspw];
	      if (celltocheck!=0)
		if (thiscell>celltocheck)
		  {
		    bin->priv->hsp[ro-1+theta*bin->priv->hspw] = bin->priv->hsp[ro-1+theta*bin->priv->hspw] + abs(bin->priv->hsp[ro+theta*bin->priv->hspw]-bin->priv->hsp[ro-1+theta*bin->priv->hspw])/2;
		  }
	      if (theta>0)
		{
		  celltocheck = bin->priv->hsp[ro-1+(theta-1)*bin->priv->hspw];
		  if (celltocheck!=0)
		    if (thiscell>celltocheck)
		      {
			bin->priv->hsp[ro-1+(theta-1)*bin->priv->hspw] = bin->priv->hsp[ro-1+(theta-1)*bin->priv->hspw] + abs(bin->priv->hsp[ro+theta*bin->priv->hspw]-bin->priv->hsp[ro-1+(theta-1)*bin->priv->hspw])/2;
		      }
		}
	      if (theta<bin->priv->hsph-1)
		{
		  celltocheck = bin->priv->hsp[ro-1+(theta+1)*bin->priv->hspw];
		  if (celltocheck!=0)
		    if (thiscell>celltocheck)
		      {
			bin->priv->hsp[ro-1+(theta+1)*bin->priv->hspw] = bin->priv->hsp[ro-1+(theta+1)*bin->priv->hspw] + abs(bin->priv->hsp[ro+theta*bin->priv->hspw]-bin->priv->hsp[ro-1+(theta+1)*bin->priv->hspw])/2;
		      }
		}
	    }
	  
	  if (ro<bin->priv->hspw)
	    {
	      celltocheck = bin->priv->hsp[ro+1+theta*bin->priv->hspw];
	      if (celltocheck!=0)
		if (thiscell>celltocheck)
		  {
		    bin->priv->hsp[ro+1+theta*bin->priv->hspw] = bin->priv->hsp[ro+1+theta*bin->priv->hspw] + abs(bin->priv->hsp[ro+theta*bin->priv->hspw]-bin->priv->hsp[ro+1+theta*bin->priv->hspw])/2;
		  }
	      if (theta>0)
		{
		  celltocheck = bin->priv->hsp[ro+1+(theta-1)*bin->priv->hspw];
		  if (celltocheck!=0)
		    if (thiscell>celltocheck)
		      {
			bin->priv->hsp[ro+1+(theta-1)*bin->priv->hspw] = bin->priv->hsp[ro+1+(theta-1)*bin->priv->hspw] + abs(bin->priv->hsp[ro+theta*bin->priv->hspw]-bin->priv->hsp[ro+1+(theta-1)*bin->priv->hspw])/2;
		      }
		}
	      if (theta<bin->priv->hsph-1)
		{
		  celltocheck = bin->priv->hsp[ro+1+(theta+1)*bin->priv->hspw];
		  if (celltocheck!=0)
		    if (thiscell>celltocheck)
		      {
			bin->priv->hsp[ro+1+(theta+1)*bin->priv->hspw] = bin->priv->hsp[ro+1+(theta+1)*bin->priv->hspw] + abs(bin->priv->hsp[ro+theta*bin->priv->hspw]-bin->priv->hsp[ro+1+(theta+1)*bin->priv->hspw])/2;
		      }
		}
	    }
	  if (theta>0)
	    {
	      celltocheck = bin->priv->hsp[ro+(theta-1)*bin->priv->hspw];
	      if (celltocheck!=0)
		if (thiscell>celltocheck)
		  {
		    bin->priv->hsp[ro+(theta-1)*bin->priv->hspw] = bin->priv->hsp[ro+(theta-1)*bin->priv->hspw] + abs(bin->priv->hsp[ro+theta*bin->priv->hspw]-bin->priv->hsp[ro+(theta-1)*bin->priv->hspw])/2;
		  }
	    }
	  if (theta<bin->priv->hsph-1)
	    {
	      celltocheck = bin->priv->hsp[ro+(theta+1)*bin->priv->hspw];
	      if (celltocheck!=0)
		if (thiscell>celltocheck)
		  {
		    bin->priv->hsp[ro+(theta+1)*bin->priv->hspw] = bin->priv->hsp[ro+(theta+1)*bin->priv->hspw] + abs(bin->priv->hsp[ro+theta*bin->priv->hspw]-bin->priv->hsp[ro+(theta+1)*bin->priv->hspw])/2;
		  }
	    }
	}
    }
}

void convhoughsmoothmean (EcvFilter_Convhough *bin)
{
  int ro;
  unsigned int theta;
  unsigned int thiscell = 0;
  unsigned int celltocheck = 0;
  for (theta=0;theta<bin->priv->hsph;theta=theta+1)
    {
      for (ro=0;ro<bin->priv->hspw;ro=ro+1)
	{
	  //check every cell around
	  thiscell = bin->priv->hsp[ro+theta*bin->priv->hspw];
	  //check previous in this 
	  celltocheck = bin->priv->hsp[ro+theta*bin->priv->hspw];

	  if (ro>0)
	    {
	      celltocheck = bin->priv->hsp[ro-1+theta*bin->priv->hspw];
	      if (celltocheck!=0)
		if (thiscell>celltocheck)
		  {
		    bin->priv->hsp[ro-1+theta*bin->priv->hspw] = bin->priv->hsp[ro+theta*bin->priv->hspw]*2/3;
		    if (bin->priv->hsp[ro-1+theta*bin->priv->hspw]==0) 
		      // the number was too little
		      bin->priv->hsp[ro-1+theta*bin->priv->hspw]=1;
		  }
	      if (theta>0)
		{
		  celltocheck = bin->priv->hsp[ro-1+(theta-1)*bin->priv->hspw];
		  if (celltocheck!=0)
		    if (thiscell>celltocheck)
		      {
			bin->priv->hsp[ro-1+(theta-1)*bin->priv->hspw] = bin->priv->hsp[ro+theta*bin->priv->hspw]*2/3;
			if (bin->priv->hsp[ro-1+(theta-1)*bin->priv->hspw]==0)
			  bin->priv->hsp[ro-1+(theta-1)*bin->priv->hspw]=1;
		      }
		}
	      if (theta<bin->priv->hsph-1)
		{
		  celltocheck = bin->priv->hsp[ro-1+(theta+1)*bin->priv->hspw];
		  if (celltocheck!=0)
		    if (thiscell>celltocheck)
		      {
			bin->priv->hsp[ro-1+(theta+1)*bin->priv->hspw] = bin->priv->hsp[ro+theta*bin->priv->hspw]*2/3;
			if (bin->priv->hsp[ro-1+(theta+1)*bin->priv->hspw]==0)
			  bin->priv->hsp[ro-1+(theta+1)*bin->priv->hspw]=1;
		    
		      }
		}
	    }
	  
	  if (ro<bin->priv->hspw)
	    {
	      celltocheck = bin->priv->hsp[ro+1+theta*bin->priv->hspw];
	      if (celltocheck!=0)
		if (thiscell>celltocheck)
		  {
		    bin->priv->hsp[ro+1+theta*bin->priv->hspw] = bin->priv->hsp[ro+theta*bin->priv->hspw]*2/3;
		    if (bin->priv->hsp[ro+1+theta*bin->priv->hspw]==0)
		      bin->priv->hsp[ro+1+theta*bin->priv->hspw]=1;
		  }
	      if (theta>0)
		{
		  celltocheck = bin->priv->hsp[ro+1+(theta-1)*bin->priv->hspw];
		  if (celltocheck!=0)
		    if (thiscell>celltocheck)
		      {
			bin->priv->hsp[ro+1+(theta-1)*bin->priv->hspw] = bin->priv->hsp[ro+theta*bin->priv->hspw]*2/3;
			if (bin->priv->hsp[ro+1+(theta-1)*bin->priv->hspw]==0)
			  bin->priv->hsp[ro+1+(theta-1)*bin->priv->hspw]=1;
		      }
		}
	      if (theta<bin->priv->hsph-1)
		{
		  celltocheck = bin->priv->hsp[ro+1+(theta+1)*bin->priv->hspw];
		  if (celltocheck!=0)
		    if (thiscell>celltocheck)
		      {
			bin->priv->hsp[ro+1+(theta+1)*bin->priv->hspw] = bin->priv->hsp[ro+theta*bin->priv->hspw]*2/3;
			if (bin->priv->hsp[ro+1+(theta+1)*bin->priv->hspw]==0)
			  bin->priv->hsp[ro+1+(theta+1)*bin->priv->hspw]=1;
		      }
		}
	    }
	  if (theta>0)
	    {
	      celltocheck = bin->priv->hsp[ro+(theta-1)*bin->priv->hspw];
	      if (celltocheck!=0)
		if (thiscell>celltocheck)
		  {
		    bin->priv->hsp[ro+(theta-1)*bin->priv->hspw] = bin->priv->hsp[ro+theta*bin->priv->hspw]*2/3;
		    if (bin->priv->hsp[ro+(theta-1)*bin->priv->hspw]==0)
		      bin->priv->hsp[ro+(theta-1)*bin->priv->hspw]=1;
		  }
	    }
	  if (theta<bin->priv->hsph-1)
	    {
	      celltocheck = bin->priv->hsp[ro+(theta+1)*bin->priv->hspw];
	      if (celltocheck!=0)
		if (thiscell>celltocheck)
		  {
		    bin->priv->hsp[ro+(theta+1)*bin->priv->hspw] = bin->priv->hsp[ro+theta*bin->priv->hspw]*2/3;
		    if (bin->priv->hsp[ro+(theta+1)*bin->priv->hspw]==0)
		      bin->priv->hsp[ro+(theta+1)*bin->priv->hspw]=1;

		  }
	    }
	}
    }
}


void 
convhoughprocess (EcvFilter *self)
{
  //  switch (self->priv->bpp)
  //    {
  //      while ()
  //	self->priv->data
  EcvFilter_Convhough * bin = ECV_FILTER_CONVHOUGH(self);
  memset ((char*)bin->priv->hsp,0,bin->priv->hspw*bin->priv->hsph*sizeof(int));
  //  g_print ("calling plugged process\n");
  int px;
  unsigned char * data = ecv_filter_get_data(self);
  int datasz = ecv_filter_get_data_size(self);

  int width;
  int height;
  g_object_get (G_OBJECT (bin),"width", &width, NULL);
  g_object_get (G_OBJECT (bin),"height", &height, NULL);

  //1: count average for each box
  int i = 0;
  int j = 0;

  int ro;
  unsigned int theta;
  double radtheta;
  
  //build theta-ro map
  for (j;j<height;j=j+1)
    {
      for (i=0;i<width*3;i=i+3)
	{
	  px = data[j*width*3 + i];
	  if (px==0x00)
	    {
	      for (theta=0;theta<180;theta=theta+1)
		{
		  //		  radtheta = ((double)(theta))*M_PI/180.0;
		  ro = bin->priv->costable[theta]*i/3+bin->priv->sintable[theta]*j;
		  bin->priv->hsp[width+ro+theta*bin->priv->hspw] = bin->priv->hsp[width+ro+theta*bin->priv->hspw] + 1;
		  //		  printf ("i/3=%i, j=%i, ro=%i radtheta=%f, theta = %i, votes=%i\n", i/3,j,ro, radtheta, theta, bin->priv->hsp[width+ro+theta*bin->priv->hspw]);
		}
	    }
	}
    }

}

void 
convhoughcreate (EcvFilter *self)
{
  EcvFilter_Convhough * bin = ECV_FILTER_CONVHOUGH(self);
  int width;
  int height;
  g_object_get (G_OBJECT (bin),"width", &width, NULL);
  g_object_get (G_OBJECT (bin),"height", &height, NULL);

  bin->priv->lines=NULL;

  bin->priv->hspw = (int)(width*cos(0.785398164)+height*sin(0.785398164))+width;
  bin->priv->hsph = 180;
  
  bin->priv->hsp = malloc (bin->priv->hspw*bin->priv->hsph*sizeof (int));
  memset ((char*)bin->priv->hsp,0,bin->priv->hspw*bin->priv->hsph*sizeof(int));

  double radtheta;
  int theta;
  int ro;

  bin->priv->costable = malloc (sizeof (double)*180);
  bin->priv->sintable = malloc (sizeof (double)*180);

  for (theta=0; theta<180; theta=theta+1)
    {
      radtheta = ((double)(theta))*M_PI/180.0;
      bin->priv->costable[theta]=cos(radtheta);
      bin->priv->sintable[theta]=sin(radtheta);
    }
  
  /*
  for (theta=0;theta<bin->priv->hsph;theta=theta+1)
    {
      for (ro=0;ro<bin->priv->hspw;ro=ro+1)
	{
	  bin->priv->hsp[ro+theta*bin->priv->hspw]=0;
	}
    }
  */
    
}

/*static void
ecv_filter_convhough_set_property (GObject      *object,
                        guint         property_id,
                        const GValue *value,
                        GParamSpec   *pspec)
{
  EcvFilter_Convhough *self = ECV_FILTER_CONVHOUGH (object);

  switch (property_id)
    {
    case PROP_ECV_XDIV:
      self->priv->xdiv = g_value_get_uint (value);
      break;

    case PROP_ECV_YDIV:
      self->priv->ydiv = g_value_get_uint (value);
      break;

    default:
      G_OBJECT_WARN_INVALID_PROPERTY_ID (object, property_id, pspec);
      break;
    }
}

static void
ecv_filter_convhough_get_property (GObject    *object,
                        guint       property_id,
                        GValue     *value,
                        GParamSpec *pspec)
{
  EcvFilter_Convhough *self = ECV_FILTER_CONVHOUGH (object);

  switch (property_id)
    {
    case PROP_ECV_XDIV:
      g_value_set_uint (value, self->priv->xdiv);
      break;

    case PROP_ECV_YDIV:
      g_value_set_uint (value, self->priv->ydiv);
      break;

    default:
      G_OBJECT_WARN_INVALID_PROPERTY_ID (object, property_id, pspec);
      break;
      }
}
*/
static void
ecv_filter_convhough_dispose (GObject *gobject)
{
  EcvFilter_Convhough *self = ECV_FILTER_CONVHOUGH (gobject);

  /* 
   * In dispose, you are supposed to free all types referenced from this
   * object which might themselves hold a reference to self. Generally,
   * the most simple solution is to unref all members on which you own a 
   * reference.
   */

  /* dispose might be called multiple times, so we must guard against
   * calling g_object_unref() on an invalid GObject.
   */

  /*  if (self->priv->)
    {
      g_object_unref (self->priv->an_object);
      }*/

  /* Chain up to the parent class */
  G_OBJECT_CLASS (ecv_filter_convhough_parent_class)->dispose (gobject);
}

static void
ecv_filter_convhough_finalize (GObject *gobject)
{
  EcvFilter_Convhough *self = ECV_FILTER_CONVHOUGH (gobject);

  if (self->priv->hsp != NULL)
    {
      free(self->priv->hsp);
      self->priv->hsp=NULL;
    }
  if (self->priv->costable!=NULL)
    {
      free(self->priv->costable);
      self->priv->costable=NULL;
    }
  if (self->priv->sintable!=NULL)
    {
      free(self->priv->sintable);
      self->priv->sintable=NULL;
    }
  /* Chain up to the parent class */
  G_OBJECT_CLASS (ecv_filter_convhough_parent_class)->finalize (gobject);
}

static GObject *
ecv_filter_convhough_constructor (GType                  gtype,
                       guint                  n_properties,
                       GObjectConstructParam *properties)
{
  GObject *obj;

  {
    /* Always chain up to the parent constructor */
    EcvFilter_ConvhoughClass *klass;
    GObjectClass *parent_class;  
    parent_class = G_OBJECT_CLASS (ecv_filter_convhough_parent_class);
    obj = parent_class->constructor (gtype, n_properties, properties);
  }
  
  /* update the object state depending on constructor properties */

  return obj;
}

static void
ecv_filter_convhough_class_init (EcvFilter_ConvhoughClass *klass)
{
  GObjectClass *gobject_class = G_OBJECT_CLASS (klass);

  GParamSpec *pspec;
  
  gobject_class->dispose = ecv_filter_convhough_dispose;
  gobject_class->finalize = ecv_filter_convhough_finalize;
  /*  gobject_class->set_property = ecv_filter_convhough_set_property;
  gobject_class->get_property = ecv_filter_convhough_get_property;

  pspec = g_param_spec_uint ("xdiv",
			     "",
			     "x divider",
			     1,
			     65534,
			     4,
			     G_PARAM_CONSTRUCT_ONLY | G_PARAM_READWRITE);
  
  g_object_class_install_property (gobject_class,
                                   PROP_ECV_XDIV,
                                   pspec);

  pspec = g_param_spec_uint ("ydiv",
			     "",
			     "y divider",
			     1,
			     65534,
			     4,
			     G_PARAM_READWRITE);
  g_object_class_install_property (gobject_class,
                                   PROP_ECV_YDIV,
                                   pspec);

  */
  EcvFilterClass *parent_class;  
  parent_class = ECV_FILTER_CLASS (klass);
  parent_class->process = convhoughprocess;
  parent_class->create = convhoughcreate;
  
  g_type_class_add_private (klass, sizeof (EcvFilter_ConvhoughPrivate));
  gobject_class->constructor = ecv_filter_convhough_constructor;
}

void ecv_filter_convhough_savepng(EcvFilter_Convhough *self, char * fname)
{
  g_return_if_fail (ECV_IS_FILTER_CONVHOUGH (self));

  int width;
  int height;
  g_object_get (G_OBJECT (self),"width", &width, NULL);
  g_object_get (G_OBJECT (self),"height", &height, NULL);

  gdImagePtr im;
  FILE * png;
  int black;
  int white;
  im = gdImageCreate(width, height);
  black = gdImageColorAllocate(im, 0, 0, 0);
  white = gdImageColorAllocate(im, 255, 255, 255);  
  gdImageFilledRectangle(im, 0, 0, width, height, white);

  unsigned int dword;
  unsigned short word;
  unsigned char byte;

  unsigned int headersize = 1+1+4+2+2+4+40;

  FILE *pngout;
  pngout = fopen(fname, "wb");

  byte = 0x42;
  fwrite(&byte,1,1,pngout);
  byte = 0x4D;
  fwrite(&byte,1,1,pngout);

  dword = self->priv->hsph*self->priv->hspw*4+headersize;
  printf ("dword = %i, w=%i,h=%i\n", dword,self->priv->hspw,self->priv->hsph);
  fwrite(&dword,4,1,pngout);
  word = 0;
  fwrite(&word,2,1,pngout);
  fwrite(&word,2,1,pngout);
  dword = 0x36;
  fwrite(&dword,4,1,pngout);

  //bitmap infoheader
  dword = 40;
  fwrite(&dword,4,1,pngout);
  dword = self->priv->hspw;
  fwrite(&dword,4,1,pngout);
  dword = self->priv->hsph;
  fwrite(&dword,4,1,pngout);
  word = 1;
  fwrite(&word,2,1,pngout);
  word = 32;
  fwrite(&word,2,1,pngout);
  dword = 0;
  fwrite(&dword,4,1,pngout);
  dword = self->priv->hsph*self->priv->hspw*4;
  fwrite(&dword,4,1,pngout);
  dword = 0x0B13;
  fwrite(&dword,4,1,pngout);
  dword = 0x0b13;
  fwrite(&dword,4,1,pngout);
  dword = 0;
  fwrite(&dword,4,1,pngout);
  dword = 0;
  fwrite(&dword,4,1,pngout);

  int px;
  int pxc;
  int ro;
  int theta;
  int max = 0;
  
  for (theta=0;theta<self->priv->hsph;theta=theta+1)
    {
      for (ro=0;ro<self->priv->hspw;ro=ro+1)
	{
	  px = self->priv->hsp[ro+theta*self->priv->hspw];
	  if (max<px)
	    max = px;
	}
    }

  unsigned char cr;
  unsigned char cg;
  unsigned char cb;
  unsigned char ca;
  //double coeff = 255.0/((double)max);
  int i = 0;
  double h;
  int x2;
  int x3;
  double dx2;
  double dy;
  for (theta=0;theta<self->priv->hsph;theta=theta+1)
    {
      for (ro=0;ro<self->priv->hspw;ro=ro+1)
	{
	  px = self->priv->hsp[ro+theta*self->priv->hspw];
	  cr = px;
	  cg = px;
	  cb = px;
	  if (px<max/2 && px > 0)
	    {
	      cr = 0x20;
	      cg = 0x20;
	      cb = 0x20;
	    }
	  else
	    {
	      dx2 = ((double)ro-width)/self->priv->costable[theta];
	      dy = dx2-height*self->priv->sintable[theta]/self->priv->costable[theta];
	      x2=(int)dx2;
	      x3=(int)dy;
	      gdImageLine(im, x2, 0, x3, height, black);
	      //	      printf ("theta=%i, ro=%i, x2=%f, y3=%f\n",theta, ro, dx2,dy);
	    }

	  if (px>255)
	    {
	      cr=255;
	      cg=255;
	      cb=255;
	    }
	  ca=0;
	  fwrite(&cr, 1,1,pngout);
	  i++;
	  fwrite(&cg, 1,1,pngout);
	  i++;
	  fwrite(&cb, 1,1,pngout);
	  i++;
	  fwrite(&ca, 1,1,pngout);
	  i++;

	}
    }

  printf ("max = %i, i = %i\n",max,i);  


  fclose(pngout);
  png = fopen("hough.png", "wb");

  gdImagePng(im, png);
  fclose(png);
  gdImageDestroy(im);
}

void convhough_line(EcvFilter_Convhough *self, int mx0, int my0, int mx1, int my1, unsigned char cr, unsigned char cg, unsigned char cb)
{
  int width;
  int height;
  unsigned char * data = ecv_filter_get_data(ECV_FILTER(self));
  g_object_get (G_OBJECT (self),"width", &width, NULL);
  g_object_get (G_OBJECT (self),"height", &height, NULL);

  double x0 = mx0;
  double y0 = my0;
  double x1 = mx1;
  double y1 = my1;

  double dx = x1 - x0;
  double dy = y1 - y0;
  
  if (fabs(dx) > fabs(dy)) 
    {          // slope < 1
      double m = dy / dx;      // compute slope
      double b = y0 - m*x0;
      dx = (dx < 0) ? -1 : 1;
      while (x0 != x1) 
	{
	  x0 += dx;
	  if (x0>=0 && x0<width && m*x0 + b>=0 && m*x0 + b<height)
	    {
	      int tx = (int)x0;
	      int ty = (int)(m*x0 + b);
	      data[(int)(tx*3+ty*width*3)]=cb;
	      data[(int)(tx*3+1+ty*width*3)]=cg;
	      data[(int)(tx*3+2+ty*width*3)]=cr;
	    }
	}
    }
  else
    {
      if (dy != 0) 
	{                              // slope >= 1
	  double m = (float) dx / (float) dy;      // compute slope
	  double b = x0 - m*y0;
	  dy = (dy < 0) ? -1 : 1;
	  while (y0 != y1) 
	    {
	      y0 += dy;
	      if (m*y0 + b>=0 && m*y0 + b<width && y0>=0 && y0<height)
		{
		  int tx = (int)(m*y0 + b);
		  int ty = (int)y0;
		  data[(int)(tx*3+ty*width*3)]=cb;
		  data[(int)(tx*3+1+ty*width*3)]=cg;
		  data[(int)(tx*3+2+ty*width*3)]=cr;
		}
            }
        }
    }
}

int convhoughcheckpxmax(EcvFilter_Convhough *self, int ro, int theta)
{
  int px;
  int pxtocheck;
  px = self->priv->hsp[ro+theta*self->priv->hspw];
  if (px == 0)
    return 0;

  if (ro>0)
    {
      pxtocheck = self->priv->hsp[ro-1+theta*self->priv->hspw];
      if (pxtocheck>px)
	{
	  return 0;
	}
      if (theta>0)
	{
	  pxtocheck = self->priv->hsp[ro-1+(theta-1)*self->priv->hspw];
	  if (pxtocheck>px)
	    {
	      return 0;
	    }
	}
      if (theta<self->priv->hsph-1)
	{
	  pxtocheck = self->priv->hsp[ro-1+(theta+1)*self->priv->hspw];
	  if (pxtocheck>px)
	    {
	      return 0;
	    }
	}
    }
  if (ro<self->priv->hspw)
    {
      pxtocheck = self->priv->hsp[ro+1+theta*self->priv->hspw];
      if (pxtocheck>px)
	{
	  return 0;
	}
      if (theta>0)
	{
	  pxtocheck = self->priv->hsp[ro+1+(theta-1)*self->priv->hspw];
	  if (pxtocheck>px)
	    {
	      return 0;
	    }
	}
      if (theta<self->priv->hsph-1)
	{
	  pxtocheck = self->priv->hsp[ro+1+(theta+1)*self->priv->hspw];
	  if (pxtocheck>px)
	    {
	      return 0;
	    }
	}
    }
  if (theta>0)
    {
      pxtocheck = self->priv->hsp[ro+(theta-1)*self->priv->hspw];
      if (pxtocheck>px)
	{
	  return 0;
	}
    }
  if (theta<self->priv->hsph-1)
    {
      pxtocheck = self->priv->hsp[ro+(theta+1)*self->priv->hspw];
      if (pxtocheck>px)
	{
	  return 0;
	}
    }
  return 1;
}
void convhoughthreshold(EcvFilter_Convhough * self, int thr)
{
  int ro;
  int theta;
  int px;
  for (theta=0;theta<self->priv->hsph;theta=theta+1)
    {
      for (ro=0;ro<self->priv->hspw;ro=ro+1)
	{
	  px = self->priv->hsp[ro+theta*self->priv->hspw];
	  if (px<thr)
	    self->priv->hsp[ro+theta*self->priv->hspw]=0;
	}
    }
}

GHashTable * ecv_filter_convhough_getlines(EcvFilter_Convhough *self)
{
  g_return_if_fail (ECV_IS_FILTER_CONVHOUGH (self));
  
  return self->priv->lines;
}

void ecv_filter_convhough_preparetodraw(EcvFilter_Convhough *self)
{
  g_return_if_fail (ECV_IS_FILTER_CONVHOUGH (self));

  int width;
  int height;
  g_object_get (G_OBJECT (self),"width", &width, NULL);
  g_object_get (G_OBJECT (self),"height", &height, NULL);

  int px;
  int pxc;
  int ro;
  int theta;
  int max = 0;
  
  for (theta=0;theta<self->priv->hsph;theta=theta+1)
    {
      for (ro=0;ro<self->priv->hspw;ro=ro+1)
	{
	  px = self->priv->hsp[ro+theta*self->priv->hspw];
	  if (max<px)
	    max = px;
	}
    }

  int thr = max*2/3;
  convhoughsmooth(self);
  convhoughthreshold(self, thr);
  convhoughfindclustersradius (self, 15);
  //  convhoughsmooth(self);
  
  unsigned char cr;
  unsigned char cg;
  unsigned char cb;
  unsigned char ca;
  //double coeff = 255.0/((double)max);
  int i = 0;
  double h;
  int x2;
  int x3;
  double dx2;
  double dy;

  //smooth
  //  convhoughsmooth(self);
  //  convhoughsmoothmean(self);
  //  convhoughsmoothmean(self);

  for (theta=0;theta<self->priv->hsph;theta=theta+1)
    {
      for (ro=0;ro<self->priv->hspw;ro=ro+1)
	{
	  if (convhoughcheckpxmax(self, ro, theta)==1)
	    {
	      dx2 = ((double)ro-width)/self->priv->costable[theta];
	      dy = dx2-height*self->priv->sintable[theta]/self->priv->costable[theta];
	      x2=(int)dx2;
	      x3=(int)dy;
	      convhough_line(self, x2, 0, x3, height-1, 0xFF, 0x55, 0x55);
	    }
	}
    }
}

static void
ecv_filter_convhough_init (EcvFilter_Convhough *self)
{
  self->priv = ECV_FILTER_CONVHOUGH_GET_PRIVATE (self);

  //  self->priv->an_object = g_object_new (ECV_TYPE_BA, NULL);
}

//------------------------------------------------------
/*                     methods                        */

EcvFilter_Convhough * 
ecv_filter_convhough_new (int width, int height, int bpp)
{
  return g_object_new (ECV_TYPE_FILTER_CONVHOUGH,
		       "width", width,
		       "height", height,
		       "bpp", bpp,
		       NULL);
}
